//
// Created by juangeng on 17/07/19.
//

#ifndef TRACKINGTEST_ORITRANS_H
#define TRACKINGTEST_ORITRANS_H

#include <Eigen/Dense>
#include <math.h>

//#define D2R M_PI/180.0                                    //弧度转度
//#define R2D 180.0/ M_PI

class oriTrans{
public:
    void Euler2DCM(Eigen::Vector3d Euler, Eigen::Matrix3d *DCM) {
        double pitch = Euler(0), roll = Euler(1), yaw = Euler(2);
        double c_11 = cos(roll)*cos(yaw);
        double c_12 = -cos(pitch)*sin(yaw) + sin(pitch)*sin(roll)*cos(yaw);
        double c_13 = sin(pitch)*sin(yaw) + cos(pitch)*sin(roll)*cos(yaw);
        double c_21 = cos(roll)*sin(yaw);
        double c_22 = cos(pitch)*cos(yaw) + sin(pitch)*sin(roll)*sin(yaw);
        double c_23 = -sin(pitch)*cos(yaw) + cos(pitch)*sin(roll)*sin(yaw);
        double c_31 = -sin(roll);
        double c_32 = sin(pitch)*cos(roll);
        double c_33 = cos(pitch)*cos(roll);
        *DCM << c_11, c_12, c_13,
                c_21, c_22, c_23,
                c_31, c_32, c_33;
    }
    //输入：　四元素，　输出：姿态矩阵，　单位：rad
    void Quater2DCM(Eigen::Quaterniond Quater, Eigen::Matrix3d *DCM) {
        double q0 = Quater.w(), q1 = Quater.x(), q2 = Quater.y(), q3 = Quater.z();
        *DCM << q0*q0 + q1*q1 - q2*q2 - q3*q3, 2*(q1*q2-q0*q3), 2*(q1*q3+q0*q2),
                2*(q1*q2+q0*q3), q0*q0 - q1*q1 + q2*q2 - q3*q3, 2*(q2*q3-q0*q2),
                2*(q1*q3-q0*q2), 2*(q2*q3+q0*q1), q0*q0 - q1*q1 - q2*q2 + q3*q3;
    }
    //输入：欧拉角，　输出：四元素，　单位：rad
    void Euler2Quater(Eigen::Vector3d Euler, Eigen::Quaterniond *Quater) {
        double e1 = Euler(0)/2, e2 = Euler(1)/2, e3 = Euler(2)/2;
        Quater->w() = cos(e1)*cos(e2)*cos(e3) + sin(e1)*sin(e2)*sin(e3);
        Quater->x() = sin(e1)*cos(e2)*cos(e3) - cos(e1)*sin(e2)*sin(e3);
        Quater->y() = cos(e1)*sin(e2)*cos(e3) + sin(e1)*cos(e2)*sin(e3);
        Quater->z() = cos(e1)*cos(e2)*sin(e3) - sin(e1)*sin(e2)*cos(e3);
    }
    //输入：姿态矩阵，　输出：欧拉角，　单位：rad
    void DCM2Euler(Eigen::Matrix3d DCM, Eigen::Vector3d *Euler) {
        //e1(-PI, PI), e2(-PI/2, PI/2), e3(-PI, PI)
        double e1 = atan2(DCM(2,1), DCM(2,2));
        double e2 = atan2(-DCM(2,0), (pow((pow(DCM(2,1),2) + pow(DCM(2,2),2)), 0.5)));
        double e3 = atan2(DCM(1,0), DCM(0,0));

        *Euler << e1, e2, e3;
    }
};

#endif //TRACKINGTEST_ORITRANS_H
